#include "com_sleepingrain_SR_NDK_LIB.h"
#include <math.h>
#include <stdlib.h>

#include "speex/speex_resampler.h"

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    multiplyAdd_Short
 * Signature: ([I[SIII)I

    public native int multiplyAdd_Short(int[] destArray,short[] addendArray,int factor,int destOffset,int addendSize);
    
 */

JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_multiplyAdd_1Short
  (JNIEnv* env, jclass obj, jintArray destArray, jshortArray addendArray, jint destOffset,jint destStride, jint addendOffset,jint addendCount,jint factor)
{
	jint* c_destArray = (*env)->GetIntArrayElements(env, destArray, 0);
	jshort* c_addendArray = (*env)->GetShortArrayElements(env, addendArray, 0);
	if ((c_destArray == 0) || (c_addendArray == 0)) {
		return -1;
	}

	jint j = destOffset;
	jint i = addendOffset;
	jint k = addendOffset+addendCount;
	
	if (destStride == 1)
	{
	    for (;i<k;++i)
	    {
		    c_destArray[j] += factor*(jint)c_addendArray[i];
		    j += 1;
	    }
	} else {
	    for (;i<k;++i)
	    {
		    c_destArray[j] += factor*(jint)c_addendArray[i];
		    j += destStride;
	    }
	}

	// release to java
	(*env)->ReleaseIntArrayElements(env, destArray, c_destArray, 0);
	(*env)->ReleaseShortArrayElements(env, addendArray, c_addendArray, 0);

	return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    multiplyAddEx_Short
 * Signature: ([I[SIIIII)I
 * 
	static public native int multiplyAddEx_Short(int[] destArray,short[] addendArray,int destOffset,int addendOffset, int addendCount,int startFactor,int endFactor);
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_multiplyAddEx_1Short
  (JNIEnv* env, jclass obj, jintArray destArray, jshortArray addendArray, jint destOffset,jint destStride, jint addendOffset, jint addendCount, jfloat startFactor, jfloat endFactor)
{
	if (addendCount <= 0) return;
	jint* c_destArray = (*env)->GetIntArrayElements(env, destArray, 0);
	jshort* c_addendArray = (*env)->GetShortArrayElements(env, addendArray, 0);
	if ((c_destArray == 0) || (c_addendArray == 0)) {
		return -1;
	}

	jint j = destOffset;
	jint i;
	
	jfloat factor = startFactor;
	jfloat factorDelta = (endFactor-startFactor) / addendCount;
	
	jint k = addendOffset+addendCount;

	for (i=addendOffset; i<k; i++) {
		c_destArray[j] += (jint)(factor*(jfloat)c_addendArray[i]);
		factor += factorDelta;
		j += destStride;
	}

	// release to java
	(*env)->ReleaseIntArrayElements(env, destArray, c_destArray, 0);
	(*env)->ReleaseShortArrayElements(env, addendArray, c_addendArray, 0);

	return 0;
}


/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    multiplyAddFade_Int
 * Signature: ([I[IIIII)I
 
    static public native int multiplyAddFade_Int(int[] destArray,int[] addendArray,int factor,int destOffset,int addendSize,int fadeSamples);

*/

JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_multiplyAddFade_1Int
  (JNIEnv* env, jclass obj, jintArray destArray, jintArray addendArray, jint destOffset,jint addendOffset,jint addendCount,jint factor, jint fadeSamples)
{
	jint* c_destArray = (*env)->GetIntArrayElements(env, destArray, 0);
	jint* c_addendArray = (*env)->GetIntArrayElements(env, addendArray, 0);
	if ((c_destArray == 0) || (c_addendArray == 0)) {
		return -1;
	}

	jint j = destOffset;
	jint i;
	
	if  (fadeSamples > 0)
	{
	    if (2*fadeSamples > addendCount)
		fadeSamples = addendCount/2;
	    
	    float fadeStep = 1.0f / fadeSamples;
	    float fade = fadeStep;

    	    jint k1 = addendOffset+fadeSamples;
    	    jint k2 = addendOffset+addendCount-fadeSamples;
	    jint k3 = addendOffset+addendCount;

	    for (i=addendOffset; i<k1; ++i)
	    {
		    c_destArray[j] += fade*(float)(factor*c_addendArray[i]);
		    fade += fadeStep;
		    ++j;
	    }

	    for (i=k1; i<k2; ++i) {
		    c_destArray[j] += factor*c_addendArray[i];
		    ++j;
	    }
	    
	    fade = 1.0f;

	    for (i=k2; i<k3; ++i) {
		    c_destArray[j] += fade*(float)(factor*c_addendArray[i]);
		    fade -= fadeStep;
		    ++j;
	    }

	} else {
	    int k = addendOffset+addendCount;
	    for (i=addendOffset; i<k; ++i) {
		    c_destArray[j] += factor*c_addendArray[i];
		    ++j;
	    }
	}

	// release to java
	(*env)->ReleaseIntArrayElements(env, destArray, c_destArray, 0);
	(*env)->ReleaseIntArrayElements(env, addendArray, c_addendArray, 0);

	return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    moveBuffer
 * Signature: ([III)I
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_moveBuffer
  (JNIEnv* env, jclass obj, jintArray buffer, jint bufferLength, jint moveBackBySamples)
{
	jint* c_buffer = (*env)->GetIntArrayElements(env, buffer, 0);
	jint k = bufferLength-moveBackBySamples;
	memmove((void*)&c_buffer[0],(const void*)&c_buffer[moveBackBySamples],k*sizeof(jint));
	memset((void*)&c_buffer[k],0,moveBackBySamples*sizeof(jint));
	(*env)->ReleaseIntArrayElements(env, buffer, c_buffer, 0);
	return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    fillBuffer
 * Signature: ([IIII)I
 * 
static public native int fillBuffer(int[] buffer,int bufferOffset,int bufferCount,int value);
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_fillBuffer
  (JNIEnv* env, jclass obj, jintArray buffer, jint bufferOffset, jint bufferCount, jint value)
{
	jint* c_buffer = (*env)->GetIntArrayElements(env, buffer, 0);
	memset((void*)&c_buffer[bufferOffset],value,bufferCount*sizeof(jint));
	(*env)->ReleaseIntArrayElements(env, buffer, c_buffer, 0);
	return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    byte2short
 * Signature: ([B[SI)I
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_byte2short
  (JNIEnv* env, jclass obj, jbyteArray srcArray, jshortArray destArray,jint srcLength)
{
	jshort* c_destArray = (*env)->GetShortArrayElements(env, destArray, 0);
	jbyte* c_srcArray = (*env)->GetByteArrayElements(env, srcArray, 0);
	if ((c_destArray == 0) || (c_srcArray == 0)) {
		return -1;
	}
	
	jint j = 0;
	jint sampleCount = srcLength/2;
	jint i;
	for (i=0;i<sampleCount;++i)
	{
		c_destArray[i] = (short) ((c_srcArray[2*i] & 0xff) | (c_srcArray[2*i+1] << 8));
	}

	// release to java
	(*env)->ReleaseShortArrayElements(env, destArray, c_destArray, 0);
	(*env)->ReleaseByteArrayElements(env, srcArray, c_srcArray, 0);

	return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    deinterlace
 * Signature: ([S[S[SI)I
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_deinterlace
 (JNIEnv * env, jclass obj, jshortArray srcArray, jshortArray destArray1, jshortArray destArray2, jint srcSize)
{
	jshort* c_srcArray = (*env)->GetShortArrayElements(env, srcArray, 0);
	jshort* c_destArray1 = (*env)->GetShortArrayElements(env, destArray1, 0);
	jshort* c_destArray2 = (*env)->GetShortArrayElements(env, destArray2, 0);
	
	jint i = 0;
	jint j = 0;
	jint k = srcSize-1;
	while (i < k)
	{
	    c_destArray1[j] = c_srcArray[i];++i;
	    c_destArray2[j] = c_srcArray[i];++i;
	    ++j;
	}
	
	(*env)->ReleaseShortArrayElements(env, srcArray, c_srcArray, 0);
	(*env)->ReleaseShortArrayElements(env, destArray1, c_destArray1, 0);
	(*env)->ReleaseShortArrayElements(env, destArray2, c_destArray2, 0);
	return 0;
}


/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    clamp2short
 * Signature: ([III[SI)I
	// clamp2short: returns the int divisor, required to make everything fit
	public native int clamp2short(int[] srcArray,int srcOffset,int srcCount,short[] destArray,int divisor,boolean updateDivisor);
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_clamp2short
  (JNIEnv* env,jobject obj,jintArray srcArray,jint srcOffset,jint srcCount,jshortArray destArray,jint divisor,jboolean updateDivisor)
{
	jint* c_srcArray = (*env)->GetIntArrayElements(env, srcArray, 0);
	jshort* c_destArray = (*env)->GetShortArrayElements(env, destArray, 0);
	if ((c_destArray == 0) || (c_srcArray == 0)) {
		return -1;
	}

	jint result = divisor;
	if (updateDivisor)
	{
		// determine max value
		jint max_value = 0;
		jint i;
		for (i=srcOffset; i<srcCount; ++i) {
			jint v = abs(c_srcArray[i]);
			if (v > max_value)
			  max_value = v;
		}
		jint w = (jint)ceil((double)max_value/32000.0);
		if (w > result)
			result = w;
	}

	float fac = 1.0/(float)result;

	jint j = srcOffset;
	jint i;
	// clamping
	for (i=0; i<srcCount; ++i) {
		float t = fac * (float)c_srcArray[j];
		jshort s;
		if (t > 32000) {
			s = 32000;
		} else {
			if (t < -32000) {
				s = -32000;
			} else {
				s = (jshort)t;
			}
		}
		c_destArray[i] = s;
		++j;
	}

	// release to java
	(*env)->ReleaseShortArrayElements(env, destArray, c_destArray, 0);
	(*env)->ReleaseIntArrayElements(env, srcArray, c_srcArray, 0);

	return result;
}

/*
 * Class:     com_sleepingrainNDK_SR_NDK_LIB
 * Method:    resample
 * Signature: ([II[III)I
 */

JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_resample
  (JNIEnv* env, jobject obj, jintArray srcArray, jint srcLength, jintArray destArray, jint destLength, jint quality)
{
	jint* c_srcArray = (*env)->GetIntArrayElements(env, srcArray, 0);
	jint* c_destArray = (*env)->GetIntArrayElements(env, destArray, 0);
	if ((c_destArray == 0) || (c_srcArray == 0)) {
		return -1;
	}

	float dx = (float)srcLength/(float)destLength;

	// fmax : nyqist half of destination sampleRate
	// fmax / fsr = 0.5;
	float fmaxDivSR = 0.5;
	float r_g = 2 * fmaxDivSR;

	// Quality is half the window width
	int wndWidth2 = quality;
	int wndWidth = quality*2;

	float x = 0;
	int i;
	for (i=0;i<destLength;++i)
	{
		float r_y = 0.0;
		int tau;
		for (tau=-wndWidth2;tau < wndWidth2;++tau)
		{
			// input sample index
			int j = (int)(x+tau);

			// Hann Window. Scale and calculate sinc
			float r_w = 0.5 - 0.5 * cos(2*M_PI*(0.5 + (j-x)/wndWidth));
			float r_a = 2*M_PI*(j-x)*fmaxDivSR;
			float r_snc = 1.0;
			if (r_a != 0)
				r_snc = sin(r_a)/r_a;

			if ((j >= 0) && (j < srcLength))
			{
				r_y += r_g * r_w * r_snc * c_srcArray[j];
			}
		}
		c_destArray[i] = r_y;
		x += dx;
	}

	// release to java
	(*env)->ReleaseIntArrayElements(env, destArray, c_destArray, 0);
	(*env)->ReleaseIntArrayElements(env, srcArray, c_srcArray, 0);

	return 0;
}

/*

Quick & Dirty Simple Slow ReSampling technique

rem - QDSS Windowed Sinc ReSampling subroutine in Basic
rem
rem function parameters
rem : x      = new sample point location (relative to old indexes)
rem            (e.g. every other integer for 0.5x decimation)
rem : indat  = original data array
rem : alim   = size of data array
rem : fmax   = low pass filter cutoff frequency
rem : fsr    = sample rate
rem : wnwdth = width of windowed Sinc used as the low pass filter
rem
rem resamp() returns a filtered new sample point

sub resamp(x, indat, alim, fmax, fsr, wnwdth)
  local i,j, r_g,r_w,r_a,r_snc,r_y	: rem some local variables
  r_g = 2 * fmax / fsr           : rem Calc gain correction factor
  r_y = 0
  for i = -wnwdth/2 to (wnwdth/2)-1 : rem For 1 window width
	 j       = int(x + i)          : rem Calc input sample index
		  : rem calculate von Hann Window. Scale and calculate Sinc
	 r_w     = 0.5 - 0.5 * cos(2*pi*(0.5 + (j - x)/wnwdth))
	 r_a     = 2*pi*(j - x)*fmax/fsr
	 r_snc   = 1  : if (r_a <> 0) then r_snc = sin(r_a)/r_a
	 if (j >= 0) and (j < alim) then
		r_y   = r_y + r_g * r_w * r_snc * indat(j)
	 endif
  next i
  resamp = r_y                  : rem Return new filtered sample
end sub

rem  - Ron Nicholson's QDSS ReSampler cookbook recipe
rem                 QDSS = Quick, Dirty, Simple and Short
rem  Version 0.1b - 2007-Aug-01
rem  Copyright 2007 Ronald H. Nicholson Jr.
rem  No warranties implied.  Error checking, optimization, and
rem    quality assessment of the "results" is left as an exercise
rem    for the student.
rem  (consider this code Open Source under a BSD style license)
rem  IMHO. YMMV.  http://www.nicholson.com/rhn/dsp.html

Notes:

	fmax should be less than half of fsr, and less than half of new_fsr (the reciprocal of the x step size).
	Filter quality increases with a larger window width. The wider the window, the closer fmax can approach half of fsr or new_fsr.
	Several operations inside the FOR loop can be pre-calculated.
	There are more optimal windows than the von Hann window.
	If the x step size is rational the same Window and Sinc values will be recalculated repeatedly.
	Therefore these values can either be cached, or pre-calculated and stored in a table (polyphase interpolation);
	or interpolated from a smaller pre-calculated table; or computed from a set of low-order polynomials fitted to
	each section or lobe between zero-crossings of the windowed Sinc (Farrow).
	(Performance optimization is left as an exercise for the student).

*/

// SPEEX STUFF

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    speex_resampler_init
 * Signature: (JJI)J

Init returns address of sampler state struct
public native long speex_resampler_init(long in_rate,long out_rate,int quality);

*/
JNIEXPORT jlong JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_speex_1resampler_1init
  (JNIEnv* env, jobject obj,jint channelCount,jlong in_rate, jlong out_rate, jint quality)
{
    int error;
    SpeexResamplerState* state = SR_SPEEX_resampler_init(channelCount,in_rate,out_rate,quality,&error);
    if (error)
	state = 0;
    return (jlong)(int)state;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    speex_resampler_process
 * Signature: (J[SJ[SJ)I
// Process the given data and reset the state (no history)
public native int speex_resampler_process(long state,short[] inArray,long inLength, short[] outArray,long outLength);
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_speex_1resampler_1process
  (JNIEnv * env, jobject obj, jlong state, jshortArray inArray, jlong inLength, jshortArray outArray, jlong outLength)
{
	SpeexResamplerState* stateP = (SpeexResamplerState*)(int)state;
	jshort* c_inArray = (*env)->GetShortArrayElements(env, inArray, 0);
	jshort* c_outArray = (*env)->GetShortArrayElements(env, outArray, 0);
	
/** Resample an interleaved int array. The input and output buffers must *not* overlap.
 * @param st Resampler state
 * @param in Input buffer
 * @param in_len Number of input samples in the input buffer. Returns the number
 * of samples processed. This is all per-channel.
 * @param out Output buffer
 * @param out_len Size of the output buffer. Returns the number of samples written.
 * This is all per-channel.
int speex_resampler_process_interleaved_int(SpeexResamplerState *st, 
                                             const spx_int16_t *in, 
                                             spx_uint32_t *in_len, 
                                             spx_int16_t *out, 
                                             spx_uint32_t *out_len);
 */
	spx_uint32_t in_len = inLength;
	spx_uint32_t out_len = outLength;
	
	int result = SR_SPEEX_resampler_process_interleaved_int(stateP,c_inArray,&in_len,c_outArray,&out_len);
	
	(*env)->ReleaseShortArrayElements(env, inArray, c_inArray, 0);
	(*env)->ReleaseShortArrayElements(env, outArray, c_outArray, 0);
	return result;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    speex_resampler_destroy
 * Signature: (J)I
// Finalize the sampler
public native int speex_resampler_destroy(long state);
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_speex_1resampler_1destroy
  (JNIEnv* env, jobject obj, jlong state)
{
    SpeexResamplerState* stateP = (SpeexResamplerState*)(int)state;
    SR_SPEEX_resampler_destroy(stateP);
    return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    filter_lowShelf
 * Signature: ([IIJFF)I
int filter_lowShelf(int[] samples,int sampleCount,long sampleRate,float freq,float gain);

*/

//	dsp_macmv   : _accum.f = ((`3)*(`4))+ _accum : `1 = `2
//	dsp_macmv_na: _accum.f = ((`1)*(`2))+ _accum
//	dsp_accum   : `1 = _accum.f : _accum = 0

#define dsp_macmv(a1,a2,a3,a4) dsp_accum += ((a3)*(a4));a1 = a2;
#define dsp_macmv_na(a1,a2) dsp_accum += ((a1)*(a2));
#define dsp_accum(a1) a1 = dsp_accum; dsp_accum = 0.0f;

JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_filter_1lowShelf_1Stereo
  (JNIEnv * env, jclass obj, jshortArray samples, jint sampleCount, jlong sampleRate, jfloat freq, jfloat gain)
{
	jshort* c_samples = (*env)->GetShortArrayElements(env, samples, 0);
	if ((c_samples == 0)) {
		return -1;
	}
	// Calculate filter coeficients
	float eqband  = 10.0f;
	float TMP     = pow(10.0f,gain/400.0f);
	float omega   = 6.2831853f*(freq/sampleRate);
	float sn      = sin(omega);
	float cs      = cos(omega);
	float beta    = sqrt(TMP*TMP+1.0f)/(eqband/10.0f)-((TMP-1.0f)*(TMP-1.0f));
	float a0      = ((TMP+1.0f)+((TMP-1.0f)*cs)+(beta*sn));
	float scal    = a0;
	float b0      = (TMP*((TMP+1.0f)-((TMP-1.0f)*cs)+(beta*sn)))/scal;
	float b1      = (2.0f*TMP*((TMP-1.0f)-((TMP+1.0f)*cs)))/scal;
	float b2      = (TMP*((TMP+1.0f)-((TMP-1.0f)*cs)-(beta*sn)))/scal;
	float a1      = (-2.0f*((TMP-1.0f)+((TMP+1.0f)*cs)))/(scal*-1.0f);
	float a2      = ((TMP+1.0f)+((TMP-1.0f)*cs)-(beta*sn))/(scal*-1.0f);
	
	static float lx2 = 0.0f;
	static float lx1 = 0.0f;
	static float ly1 = 0.0f;
	static float ly2 = 0.0f;

	static float rx2 = 0.0f;
	static float rx1 = 0.0f;
	static float ry1 = 0.0f;
	static float ry2 = 0.0f;
	
	// Apply filter
	int j = 0;
	float dsp_accum = 0.0f;
	int i;
	for (i=0;i<sampleCount;++i)
	{
	    // Left
	    float inl = c_samples[j];
	    dsp_macmv   ( lx2 ,  lx1  ,  lx2 ,  b2  )
	    dsp_macmv   ( lx1 ,  inl  ,  lx1 ,  b1  )
	    dsp_macmv_na( inl ,  b0  )
	    dsp_macmv   ( ly2 ,  ly1  ,  ly2 ,  a2  )
	    dsp_macmv_na( ly1 ,  a1  )
	    dsp_accum   ( ly1 )
	    c_samples[j] = (short)ly1;++j;

	    // Right
	    float inr = c_samples[j];
	    dsp_macmv   ( rx2 ,  rx1  ,  rx2 ,  b2  )
	    dsp_macmv   ( rx1 ,  inr  ,  rx1 ,  b1  )
	    dsp_macmv_na( inr ,  b0  )
	    dsp_macmv   ( ry2 ,  ry1  ,  ry2 ,  a2  )
	    dsp_macmv_na( ry1 ,  a1  )
	    dsp_accum   ( ry1 )
	    c_samples[j] = (short)ry1;++j;
	}

	(*env)->ReleaseShortArrayElements(env, samples, c_samples, 0);
	return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    filter_highShelf
 * Signature: ([IIJFF)I
int filter_highShelf(int[] samples,int sampleCount,long sampleRate,float freq,float gain);
 */
JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_filter_1highShelf_1Stereo
  (JNIEnv * env, jclass obj, jshortArray samples, jint sampleCount, jlong sampleRate, jfloat freq, jfloat gain)
{
	jshort* c_samples = (*env)->GetShortArrayElements(env, samples, 0);
	if ((c_samples == 0)) {
		return -1;
	}
	// Calculate filter coeficients
	float eqband  = 10.0f;
	float TMP     = pow(10.0f,gain/400.0f);
	float omega   = 6.2831853f*(freq/sampleRate);
	float sn      = sin(omega);
	float cs      = cos(omega);
	float beta    = sqrt(TMP*TMP+1.0f)/(eqband/10.0f)-((TMP-1.0f)*(TMP-1.0f));

	float a0      = ((TMP+1.0f)-((TMP-1.0f)*cs)+(beta*sn));
	float scal    = a0;

	float b0      = (TMP*((TMP+1.0f)+(TMP-1.0f)*cs+beta*sn))/scal;
	float b1      = (-2.0f*TMP*((TMP-1)+(TMP+1.0f)*cs))/scal;
	float b2      = (TMP*((TMP+1.0f)+((TMP-1.0f)*cs)-beta*sn))/scal;
	float a1      = (2.0f*((TMP-1.0f)-(TMP+1.0f)*cs))/(scal*-1.0f);
	float a2      = ((TMP+1.0f)-((TMP-1.0f)*cs)-beta*sn)/(scal*-1.0f);

	static float lx2 = 0.0f;
	static float lx1 = 0.0f;
	static float ly1 = 0.0f;
	static float ly2 = 0.0f;

	static float rx2 = 0.0f;
	static float rx1 = 0.0f;
	static float ry1 = 0.0f;
	static float ry2 = 0.0f;
	
	// Apply filter
	int j = 0;
	float dsp_accum = 0.0f;
	int i;
	for (i=0;i<sampleCount;++i)
	{
	    // Left
	    float inl = c_samples[j];
	    dsp_macmv   ( lx2 ,  lx1  ,  lx2 ,  b2  )
	    dsp_macmv   ( lx1 ,  inl  ,  lx1 ,  b1  )
	    dsp_macmv_na( inl ,  b0  )
	    dsp_macmv   ( ly2 ,  ly1  ,  ly2 ,  a2  )
	    dsp_macmv_na( ly1 ,  a1  )
	    dsp_accum   ( ly1 )
	    c_samples[j] = (short)ly1;++j;

	    // Right
	    float inr = c_samples[j];
	    dsp_macmv   ( rx2 ,  rx1  ,  rx2 ,  b2  )
	    dsp_macmv   ( rx1 ,  inr  ,  rx1 ,  b1  )
	    dsp_macmv_na( inr ,  b0  )
	    dsp_macmv   ( ry2 ,  ry1  ,  ry2 ,  a2  )
	    dsp_macmv_na( ry1 ,  a1  )
	    dsp_accum   ( ry1 )
	    c_samples[j] = (short)ry1;++j;
	}

	(*env)->ReleaseShortArrayElements(env, samples, c_samples, 0);
	return 0;
}

/*
 * Class:     com_sleepingrain_SR_NDK_LIB
 * Method:    filter_biQuad_Stereo
 * Signature: ([SIJIFFF)I
 * 
int filter_biQuad_Stereo(short[] samples,int sampleCount,long sampleRate,int type,float freq,float bandwidth, float dbGain);

 */

typedef float smp_type;

JNIEXPORT jint JNICALL Java_com_sleepingrain_SR_1NDK_1LIB_filter_1biQuad_1Stereo
  (JNIEnv * env, jclass obj, jshortArray samples, jint sampleCount, jlong sampleRate, jint type, jfloat freq,jfloat bandwidth, jfloat dbGain)
{
	jshort* c_samples = (*env)->GetShortArrayElements(env, samples, 0);
	if ((c_samples == 0)) {
		return -1;
	}
	// Calculate filter coeficients
	smp_type A, omega, sn, cs, alpha, beta;
	smp_type a0, a1, a2, b0, b1, b2;

	// setup variables
	A = pow(10, dbGain /40);
	omega = 2 * M_PI * freq /sampleRate;
	sn = sin(omega);
	cs = cos(omega);
	alpha = sn * sinh(M_LN2 /2 * bandwidth * omega /sn);
	beta = sqrt(A + A);

	switch (type) {
	// LSH Low shelf filter
	 case 0:
	    b0 = A * ((A + 1) - (A - 1) * cs + beta * sn);
	    b1 = 2 * A * ((A - 1) - (A + 1) * cs);
	    b2 = A * ((A + 1) - (A - 1) * cs - beta * sn);
	    a0 = (A + 1) + (A - 1) * cs + beta * sn;
	    a1 = -2 * ((A - 1) + (A + 1) * cs);
	    a2 = (A + 1) + (A - 1) * cs - beta * sn;
	    break;
	// HSH High shelf filter 
	 case 1:
	    b0 = A * ((A + 1) + (A - 1) * cs + beta * sn);
	    b1 = -2 * A * ((A - 1) + (A + 1) * cs);
	    b2 = A * ((A + 1) + (A - 1) * cs - beta * sn);
	    a0 = (A + 1) - (A - 1) * cs + beta * sn;
	    a1 = 2 * ((A - 1) - (A + 1) * cs);
	    a2 = (A + 1) - (A - 1) * cs - beta * sn;
	    break;	
	}
	b0 = b0 / a0;
	b1 = b1 / a0;
	b2 = b2 / a0;
	a1 = a1 / a0;
	a2 = a2 / a0;

	// History
	static smp_type lx2 = 0.0f;
	static smp_type lx1 = 0.0f;
	static smp_type ly1 = 0.0f;
	static smp_type ly2 = 0.0f;

	static smp_type rx2 = 0.0f;
	static smp_type rx1 = 0.0f;
	static smp_type ry1 = 0.0f;
	static smp_type ry2 = 0.0f;

	// Apply filter
	int j = 0;
	int i;
	for (i=0;i<sampleCount;++i)
	{
	    // Left
	    smp_type inl = c_samples[j];
	    smp_type outl = b0*inl + b1*lx1 + b2*lx2 - a1*ly1 - a2*ly2;
	    c_samples[j] = outl;
	    // shift x1 to x2, sample to x1 
	    lx2 = lx1;lx1 = inl;
	    // shift y1 to y2, result to y1 
	    ly2 = ly1;ly1 = outl;	
	    ++j;
	    
	    // Right
	    smp_type inr = c_samples[j];
	    smp_type outr = b0*inr + b1*rx1 + b2*rx2 - a1*ry1 - a2*ry2;
	    c_samples[j] = outr;
	    // shift x1 to x2, sample to x1 
	    rx2 = rx1;rx1 = inr;
	    // shift y1 to y2, result to y1 
	    ry2 = ry1;ry1 = outr;	

	    ++j;
	}
	
	(*env)->ReleaseShortArrayElements(env, samples, c_samples, 0);
	return 0;
}
